//Zoolander robot movement library
//Author: Max Macri
#include <Arduino.h>
#include "Movement.h"
 
Movement::Movement(int LF, int LR, int RF, int RR, int LE, int RE)
{
  LM_EN = LE;
  RM_EN = RE;
  LFWD = LF;
  LREV = LR;
  RFWD = RF;
  RREV = RR;
  pinMode(LFWD,OUTPUT);
  pinMode(LREV,OUTPUT);
  pinMode(RFWD,OUTPUT);
  pinMode(RREV,OUTPUT);
  pinMode(LM_EN,OUTPUT);
  pinMode(RM_EN,OUTPUT);
  digitalWrite(LM_EN,HIGH);
  digitalWrite(RM_EN,HIGH);
  LM_FWD = false;
  LM_REV = false;
  RM_FWD = false;
  RM_REV = false;
  setMControl();
}
void Movement::setMove(int hVal, int vVal){
  rotating=false;
  if(hVal>=50-deadzone && hVal <= 50+deadzone){
    rdrop = 1.0;
    ldrop = 1.0;
    //dont move horizontally
  }else if(hVal < 50-deadzone){
    rdrop = 1.0;
    ldrop = (hVal)/(50.0-deadzone);
    if(ldrop == 0.0)
      ldrop = 0.01;
  }else if (hVal > 50.0+deadzone){
    rdrop = ((50.0-deadzone)-(hVal-(50.0+deadzone)))/(50.0-deadzone);
    ldrop = 1.0;
    if(rdrop == 0.0)
      rdrop = 0.01;
  }
  if(vVal>=50-deadzone && vVal <= 50+deadzone){
    //dont move horizontally
    if(rdrop < ldrop){
      if(rdrop == 0.0)
        rdrop = 0.01;
      ldrop = 0.0-rdrop;
      dutySpeed = 255;
      rotating = true;
    }else if(ldrop < rdrop){
      if(ldrop == 0.0)
        ldrop = 0.01;
      rdrop = 0.0-ldrop;
      dutySpeed = 255;
      rotating = true;
    }else{
      dutySpeed = 0;
    }
  }else if(vVal < 50-deadzone){
    dutySpeed = map(vVal,0,(50-deadzone),255,1);
  }else if (vVal > 50+deadzone){
    dutySpeed = map(vVal,(50+deadzone),100,-1,-255);
  }
  if(rotating){
  if(rdrop >= 0){
    rdrop = map(rdrop*10,0,10,5,10);
  }else{
    rdrop = map(rdrop*10,0,-10,-5,-10);
  }
  if(ldrop >= 0){
    ldrop = map(ldrop*10,0,10,5,10);
  }else{
    ldrop = map(ldrop*10,0,-10,-5,-10);
  }
  rdrop = rdrop/10.0;
  ldrop = ldrop/10.0;
  }

  LOld=LSpeed;
  ROld=RSpeed;
  if(dutySpeed == 0){
    LSpeed = dutySpeed;
    RSpeed = dutySpeed;
  }else if(rotating){
    if(rdrop >= 0)
      RSpeed = 255-(dutySpeed*rdrop);
    else
      RSpeed = (0-255)-(dutySpeed*rdrop);
    if(ldrop >= 0)
      LSpeed = 255-(dutySpeed*ldrop);
    else
      LSpeed = (0-255)-(dutySpeed*ldrop);
  }else{
    LSpeed = dutySpeed*ldrop;
    RSpeed = dutySpeed*rdrop;
  }
  if((LSpeed > 0 && LOld < 0)||(LSpeed < 0 && LOld > 0)){
    LSpeed = 0;
  }
  if((RSpeed > 0 && ROld < 0)||(RSpeed < 0 && ROld > 0)){
    RSpeed = 0;
  }
  
  if(LSpeed==0){
    LM_FWD = false;
    LM_REV = false;
  }else if(LOld == 0){
    if(LSpeed > 0){
      LM_FWD = true;
      LM_REV = false;
    }else if(LSpeed < 0){
      LM_FWD = false;
      LM_REV = true;
    }
  }
  //if positivity changed then swap signals
  if(RSpeed==0){
    RM_FWD = false;
    RM_REV = false;
  }else if(ROld == 0){
    if(RSpeed > 0){
      RM_FWD = true;
      RM_REV = false;
    }else if(RSpeed < 0){
      RM_FWD = false;
      RM_REV = true;
    }
  }
  //if positivity changed then swap signals
  analogWrite(LM_EN,abs(LSpeed));
  analogWrite(RM_EN,abs(RSpeed));
  setMControl();
}
void Movement::setMControl(){
  if(LM_FWD)
    digitalWrite(LFWD,HIGH);
  else
    digitalWrite(LFWD,LOW);
  if(LM_REV)
    digitalWrite(LREV,HIGH);
  else
    digitalWrite(LREV,LOW);
  if(RM_FWD)
    digitalWrite(RFWD,HIGH);
  else
    digitalWrite(RFWD,LOW);
  if(RM_REV)
    digitalWrite(RREV,HIGH);
  else
    digitalWrite(RREV,LOW);
}